#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>

int main()
{   
    cv::Mat img;
    cv::Ptr<cv::aruco::Dictionary> dict;
    int markerId;
    int arucoId;
    double marker_length;
    std::string save_path;

    std::string config_path;
    config_path = "/home/lin/ros_code/aruco_ws/src/aruco_location/config/generation.yaml";
    
    // n.param<std::string>("config_path", config_path, "src/aruco_location/config/param.yaml");
    cv::FileStorage fs(config_path, cv::FileStorage::READ);
    
    markerId = fs["markerId"];
    marker_length = fs["marker_length"];
    arucoId = fs["arucoId"];
    fs["save_path"] >> save_path;
    
    std::cout << "\n"; 
    std::cout << "markerId:      " << markerId << std::endl;
    std::cout << "marker_length: " << marker_length << " pixel." << std::endl;
    std::cout << "arucoId :      " << arucoId << std::endl;
    std::cout << "save_path :    " << save_path << std::endl;
    std::cout << "\n"; 
    
    // cv::aruco::DICT_6X6_50 = 8
    dict = cv::aruco::getPredefinedDictionary(arucoId);
    cv::aruco::drawMarker(dict, markerId, marker_length, img);
    std::cout << img.size() << std::endl;
    
    cv::Mat img1;
    img.copyTo(img1);
    cv::cvtColor(img1, img1, cv::COLOR_GRAY2BGR);
    cv::putText(img1, "markerId: " + std::to_string(markerId), cv::Point2i(20,50), cv::FONT_HERSHEY_SIMPLEX, 2, cv::Scalar(0, 0, 255), 2, 8);
    cv::imshow("view", img1);
    cv::waitKey();

    cv::imwrite(save_path+"/"+ std::to_string(markerId) + ".png", img);
    std::cout << "Save image to " << save_path+"/"+ std::to_string(markerId) + ".png" << std::endl;
    return 0; 
}